#ifndef RM_SENTRY_GIMBAL_INC_INS_TASK_H_
#define RM_SENTRY_GIMBAL_INC_INS_TASK_H_

#include "stdint.h"

#include "bmi_driver.h"
#include "QuaternionEKF.h"



#define X 0
#define Y 1
#define Z 2

#define INS_TASK_PERIOD 1

typedef struct
{
    float q[4]; // 四元数估计值

    float Gyro[3];  // 角速度
    float Accel[3]; // 加速度
    float MotionAccel_b[3]; // 机体坐标加速度
    float MotionAccel_n[3]; // 绝对系加速度

    float AccelLPF; // 加速度低通滤波系数

    // 加速度在绝对系的向量表示
    float xn[3];
    float yn[3];
    float zn[3];

    float atanxz;
    float atanyz;

    // 位姿
    float Roll;
    float Pitch;
    float Yaw;
    float YawTotalAngle;
} INS_t;


typedef struct
{
    uint8_t flag;

    float scale[3];

    float Yaw;
    float Pitch;
    float Roll;
} IMU_Param_t;


float PID_Calculate_YAW(PID_t *pid, float measure, float ref);
float PID_Calculate_pitch(PID_t *pid, float measure, float ref);


void INS_Init(void);
void INS_Task(void);
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
void IMU_Temperature_Ctrl(void);
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt);
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll);
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q);
void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
void ins_init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate);

#endif
